#include "zeus_s1_driver/zeus_s1_driver.h"
#include "zeus_s1_driver/Quaternion_Solution.h"

sensor_msgs::Imu Mpu6050;   //实例化IMU对象 

/**************************************
功能: 主函数，ROS初始化，通过turn_on_robot类创建Robot_control对象并自动调用构造函数初始化
***************************************/
int main(int argc, char** argv)
{
    ros::init(argc, argv, "zeus_s1_driver");                                //ROS初始化 并设置节点名称 
    turn_on_robot Robot_Control;                                            //实例化一个控制器对象
    Robot_Control.Control();                                                //循环执行数据采集和发布话题等操作
    return 0;  
} 

/**************************************
功能: 数据转换函数
***************************************/
short turn_on_robot::IMU_Trans(uint8_t Data_High,uint8_t Data_Low)
{
    short transition_16;
    transition_16 = 0;
    transition_16 |=  Data_High<<8;   
    transition_16 |=  Data_Low;
    return transition_16;     
}

float turn_on_robot::Odom_Trans(uint8_t Data_High,uint8_t Data_Low)
{
    float data_return;
    short transition_16;
    transition_16 = 0;
    transition_16 |=  Data_High<<8;                                         //获取数据的高8位
    transition_16 |=  Data_Low;                                             //获取数据的低8位
    data_return   =  (transition_16 / 1000)+(transition_16 % 1000)*0.001;   //速度单位从mm/s转换为m/s
    return data_return;
}

/**************************************
功能: 速度话题订阅回调函数Callback，根据订阅的指令通过串口发指令控制下位机
***************************************/
void turn_on_robot::Cmd_Vel_Callback(const geometry_msgs::Twist &twist_aux)
{
    short  transition;

    Send_Data.tx[0]=FRAME_HEADER;                                           //帧头0X7B
    Send_Data.tx[1] = 0;                                                    //预留位
    Send_Data.tx[2] = 0;                                                    //预留位

    //机器人x轴的目标线速度
    transition=0;
    transition = twist_aux.linear.x*1000;                                   //将浮点数放大一千倍，简化传输
    Send_Data.tx[4] = transition;                                           //取数据的低8位
    Send_Data.tx[3] = transition>>8;                                        //取数据的高8位

    //机器人y轴的目标线速度
    transition=0;
    transition = twist_aux.linear.y*1000;
    Send_Data.tx[6] = transition;
    Send_Data.tx[5] = transition>>8;

    //机器人z轴的目标角速度
    transition=0;
    transition = twist_aux.angular.z*1000;
    Send_Data.tx[8] = transition;
    Send_Data.tx[7] = transition>>8;

    //BCC校验位，规则参见Check_Sum函数
    Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); 
    Send_Data.tx[10]=FRAME_TAIL;                                            //帧尾0X7D

    try
    {
        Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx));             //通过串口向下位机发送数据
    }
    catch (serial::IOException& e)   
    {
        ROS_ERROR_STREAM("Unable to send data through serial port");        //如果发送数据失败，打印错误信息
    }
}

/**************************************
功能: 发布IMU数据话题
***************************************/
void turn_on_robot::Publish_ImuSensor()
{
    sensor_msgs::Imu Imu_Data_Pub;                                          //实例化IMU话题数据
    Imu_Data_Pub.header.stamp = ros::Time::now(); 
    Imu_Data_Pub.header.frame_id = gyro_frame_id;  

    //四元数表达三轴姿态
    Imu_Data_Pub.orientation.x = Mpu6050.orientation.x;                       
    Imu_Data_Pub.orientation.y = Mpu6050.orientation.y; 
    Imu_Data_Pub.orientation.z = Mpu6050.orientation.z;
    Imu_Data_Pub.orientation.w = Mpu6050.orientation.w;

    //三轴姿态协方差矩阵
    Imu_Data_Pub.orientation_covariance[0] = 1e6;                             
    Imu_Data_Pub.orientation_covariance[4] = 1e6;
    Imu_Data_Pub.orientation_covariance[8] = 1e-6;

    //三轴角速度
    Imu_Data_Pub.angular_velocity.x = Mpu6050.angular_velocity.x; 
    Imu_Data_Pub.angular_velocity.y = Mpu6050.angular_velocity.y;
    Imu_Data_Pub.angular_velocity.z = Mpu6050.angular_velocity.z;

    //三轴角速度协方差矩阵
    Imu_Data_Pub.angular_velocity_covariance[0] = 1e6;
    Imu_Data_Pub.angular_velocity_covariance[4] = 1e6;
    Imu_Data_Pub.angular_velocity_covariance[8] = 1e-6;

    //三轴线性加速度
    Imu_Data_Pub.linear_acceleration.x = Mpu6050.linear_acceleration.x;
    Imu_Data_Pub.linear_acceleration.y = Mpu6050.linear_acceleration.y; 
    Imu_Data_Pub.linear_acceleration.z = Mpu6050.linear_acceleration.z; 

    //发布IMU话题 
    imu_publisher.publish(Imu_Data_Pub); 
}

/**************************************
功能: 发布里程计话题，包含位置、姿态、三轴速度、绕三轴角速度、TF父子坐标、协方差矩阵
***************************************/
void turn_on_robot::Publish_Odom()
{
    //把Z轴转角转换为四元数进行表达
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(Robot_Pos.Z);

    nav_msgs::Odometry odom;
    odom.header.stamp = ros::Time::now(); 
    odom.header.frame_id = odom_frame_id;
    odom.pose.pose.position.x = Robot_Pos.X;
    odom.pose.pose.position.y = Robot_Pos.Y;
    odom.pose.pose.position.z = Robot_Pos.Z;

    //姿态，通过Z轴转角转换的四元数
    odom.pose.pose.orientation = odom_quat;

    odom.child_frame_id = robot_frame_id;
    odom.twist.twist.linear.x =  Robot_Vel.X;                               //X方向速度
    odom.twist.twist.linear.y =  Robot_Vel.Y;                               //Y方向速度
    odom.twist.twist.angular.z = Robot_Vel.Z;                               //绕Z轴角速度 


    //这个矩阵有两种，分别在机器人静止和运动的时候使用。扩展卡尔曼滤波官方提供的2个矩阵，用于robot_pose_ekf功能包
    if((Robot_Vel.X == 0)&&(Robot_Vel.Y== 0)&&(Robot_Vel.Z==0))
    {
      //如果velocity是零，说明编码器的误差会比较小，认为编码器数据更可靠
      memcpy(&odom.pose.covariance, odom_pose_covariance2, sizeof(odom_pose_covariance2)),
      memcpy(&odom.twist.covariance, odom_twist_covariance2, sizeof(odom_twist_covariance2));
    }
    else
    {
      //如果小车velocity非零，考虑到运动中编码器可能带来的滑动误差，认为imu的数据更可靠
      memcpy(&odom.pose.covariance, odom_pose_covariance, sizeof(odom_pose_covariance)),
      memcpy(&odom.twist.covariance, odom_twist_covariance, sizeof(odom_twist_covariance));  
    } 
  
    //发布里程计话题
    odom_publisher.publish(odom);
}

/**************************************
功能: 发布电压相关信息
***************************************/
void turn_on_robot::Publish_Voltage()
{
    std_msgs::Float32 voltage_msgs;
    static float Count_Voltage_Pub=0;
    if(Count_Voltage_Pub++>10)
    {
        Count_Voltage_Pub=0;
        voltage_msgs.data = Power_voltage;                                  //电源供电的电压获取

        //发布电源电压话题单位：V、伏特
        voltage_publisher.publish(voltage_msgs); 
    }
}
/**************************************
功能: 串口通讯校验函数，数据包n有个字节，第n-1个字节为校验位，第n个字节位帧尾。第1个字节到第n-2个字节数据按位异或的结果与第n-1个字节对比，即为BCC校验
输入参数： 
  Count_Number：数据包前几个字节加入校验   
  mode：对发送数据还是接收数据进行校验
***************************************/
unsigned char turn_on_robot::Check_Sum(unsigned char Count_Number,unsigned char mode)
{
    unsigned char check_sum=0,k;
  
    if(mode==0)                                                             //接收数据模式
    {
        for(k=0;k<Count_Number;k++)
        {
            check_sum=check_sum^Receive_Data.rx[k];                         //按位异或
        }
    }
    if(mode==1)                                                             //发送数据模式
    {
        for(k=0;k<Count_Number;k++)
        {
            check_sum=check_sum^Send_Data.tx[k];                            //按位异或
        }
    }
    return check_sum;                                                       //返回按位异或结果
}

/**************************************
功能: 通过串口读取并校验下位机发送过来的数据，然后数据转换为国际单位
更新说明：该校验方法会导致出现读取错误数据或者正确数据不处理的情况，现在已不用该校验方法，换成逐帧校验方式，参考Get_Sensor_Data_New()函数
***************************************/
bool turn_on_robot::Get_Sensor_Data()
{ 
  short transition_16=0, j=0, Header_Pos=0, Tail_Pos=0; //Intermediate variable //中间变量
  static int flag_error=0,temp=1; //Static variable that records the error flag and location //静态变量，用于记录出错标志位和出错位置
  uint8_t Receive_Data_Pr[RECEIVE_DATA_SIZE]={0},Receive_Data_Tr[temp]={0}; //Temporary variable to save the data of the lower machine //临时变量，保存下位机数据
  if(flag_error==0) //Normal condition detected //检测到正常情况
    Stm32_Serial.read(Receive_Data_Pr,sizeof (Receive_Data_Pr)); //Read the data sent by the lower computer through the serial port //通过串口读取下位机发送过来的数据
  else if (flag_error==1) //Error condition detected 检测到错误情况
  {
    //Read wrong bit data through serial port, read only and do not process, so that the correct data is read next time
    //通过串口读取错位数据，只读取不处理，以便于下次读取到的是正确的数据
    Stm32_Serial.read(Receive_Data_Tr,sizeof (Receive_Data_Tr)); 
    flag_error=0; //Error flag position 0 //错误标志位置0
  }

  /*//View the received raw data directly and debug it for use//直接查看接收到的原始数据，调试使用
  ROS_INFO("%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x",
  Receive_Data_Pr[0],Receive_Data_Pr[1],Receive_Data_Pr[2],Receive_Data_Pr[3],Receive_Data_Pr[4],Receive_Data_Pr[5],Receive_Data_Pr[6],Receive_Data_Pr[7],
  Receive_Data_Pr[8],Receive_Data_Pr[9],Receive_Data_Pr[10],Receive_Data_Pr[11],Receive_Data_Pr[12],Receive_Data_Pr[13],Receive_Data_Pr[14],Receive_Data_Pr[15],
  Receive_Data_Pr[16],Receive_Data_Pr[17],Receive_Data_Pr[18],Receive_Data_Pr[19],Receive_Data_Pr[20],Receive_Data_Pr[21],Receive_Data_Pr[22],Receive_Data_Pr[23]);
  */  

  //Record the position of the head and tail of the frame //记录帧头帧尾位置
  for(j=0;j<24;j++)
  {
    if(Receive_Data_Pr[j]==FRAME_HEADER)
    Header_Pos=j;
    else if(Receive_Data_Pr[j]==FRAME_TAIL)
    Tail_Pos=j;    
  }

  if(Tail_Pos==(Header_Pos+23))
  {
    //If the end of the frame is the last bit of the packet, copy the packet directly to receive_data.rx
    //如果帧尾在数据包最后一位，直接复制数据包到Receive_Data.rx
    // ROS_INFO("1-----");
    memcpy(Receive_Data.rx, Receive_Data_Pr, sizeof(Receive_Data_Pr));
    flag_error=0; //Error flag position 0 for next reading //错误标志位置0，便于下次读取
  }
  else if(Header_Pos==(1+Tail_Pos))
  {
    //If the header is behind the end of the frame, record the position of the header so that the next reading of the error bit data can correct the data position
    //如果帧头在帧尾后面，记录帧头出现的位置，便于下次读取出错位数据以纠正数据位置
    //|********7D (7B************|**********7D) 7B************|
    // ROS_INFO("2-----");
    temp=Header_Pos; //Record the length of the next read, calculated to be exactly the position of the frame head //记录下一次读取的长度，经计算正好为帧头的位置
    flag_error=1; //Error flag position 1, error bit array for next read //错误标志位置1，让下一次读取出错位数组
    return false;
  }
  else 
  {
    return false;
  }    
  


  Receive_Data.Frame_Header= Receive_Data.rx[0]; //The first part of the data is the frame header 0X7B //数据的第一位是帧头0X7B
  Receive_Data.Frame_Tail= Receive_Data.rx[23];  //The last bit of data is frame tail 0X7D //数据的最后一位是帧尾0X7D

  if (Receive_Data.Frame_Header == FRAME_HEADER ) //Judge the frame header //判断帧头
  {
    if (Receive_Data.Frame_Tail == FRAME_TAIL) //Judge the end of the frame //判断帧尾
    { 
      if (Receive_Data.rx[22] == Check_Sum(22,READ_DATA_CHECK)) //BCC check passes or two packets are interlaced //BCC校验通过或者两组数据包交错
      {
        Receive_Data.Flag_Stop=Receive_Data.rx[1]; //set aside //预留位
        Robot_Vel.X = Odom_Trans(Receive_Data.rx[2],Receive_Data.rx[3]); //Get the speed of the moving chassis in the X direction //获取运动底盘X方向速度
        Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]); //Get the speed of the moving chassis in the Y direction, The Y speed is only valid in the omnidirectional mobile robot chassis
                                                                         //获取运动底盘Y方向速度，Y速度仅在全向移动机器人底盘有效
        Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]); //Get the speed of the moving chassis in the Z direction //获取运动底盘Z方向速度   
        
        //MPU6050 stands for IMU only and does not refer to a specific model. It can be either MPU6050 or MPU9250
        //Mpu6050仅代表IMU，不指代特定型号，既可以是MPU6050也可以是MPU9250
        Mpu6050_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8],Receive_Data.rx[9]);   //Get the X-axis acceleration of the IMU     //获取IMU的X轴加速度  
        Mpu6050_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10],Receive_Data.rx[11]); //Get the Y-axis acceleration of the IMU     //获取IMU的Y轴加速度
        Mpu6050_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12],Receive_Data.rx[13]); //Get the Z-axis acceleration of the IMU     //获取IMU的Z轴加速度
        Mpu6050_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14],Receive_Data.rx[15]);  //Get the X-axis angular velocity of the IMU //获取IMU的X轴角速度  
        Mpu6050_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16],Receive_Data.rx[17]);  //Get the Y-axis angular velocity of the IMU //获取IMU的Y轴角速度  
        Mpu6050_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18],Receive_Data.rx[19]);  //Get the Z-axis angular velocity of the IMU //获取IMU的Z轴角速度  
        //Linear acceleration unit conversion is related to the range of IMU initialization of STM32, where the range is ±2g=19.6m/s^2
        //线性加速度单位转化，和STM32的IMU初始化的时候的量程有关,这里量程±2g=19.6m/s^2
        Mpu6050.linear_acceleration.x = Mpu6050_Data.accele_x_data / ACCEl_RATIO;
        Mpu6050.linear_acceleration.y = Mpu6050_Data.accele_y_data / ACCEl_RATIO;
        Mpu6050.linear_acceleration.z = Mpu6050_Data.accele_z_data / ACCEl_RATIO;
        //The gyroscope unit conversion is related to the range of STM32's IMU when initialized. Here, the range of IMU's gyroscope is ±500°/s
        //Because the robot generally has a slow Z-axis speed, reducing the range can improve the accuracy
        //陀螺仪单位转化，和STM32的IMU初始化的时候的量程有关，这里IMU的陀螺仪的量程是±500°/s
        //因为机器人一般Z轴速度不快，降低量程可以提高精度
        Mpu6050.angular_velocity.x =  Mpu6050_Data.gyros_x_data * GYROSCOPE_RATIO;
        Mpu6050.angular_velocity.y =  Mpu6050_Data.gyros_y_data * GYROSCOPE_RATIO;
        Mpu6050.angular_velocity.z =  Mpu6050_Data.gyros_z_data * GYROSCOPE_RATIO;

        //Get the battery voltage
        //获取电池电压
        transition_16 = 0;
        transition_16 |=  Receive_Data.rx[20]<<8;
        transition_16 |=  Receive_Data.rx[21];  
        Power_voltage = transition_16/1000+(transition_16 % 1000)*0.001; //Unit conversion millivolt(mv)->volt(v) //单位转换毫伏(mv)->伏(v)

        return true;
     }
    }
  } 
  return false;
}
/**************************************
功能: 通过串口读取并逐帧校验下位机发送过来的数据，然后数据转换为国际单位
***************************************/
bool turn_on_robot::Get_Sensor_Data_New()
{
    short transition_16=0;
    uint8_t i=0,check=0, error=1,Receive_Data_Pr[1];
    static int count;

    Stm32_Serial.read(Receive_Data_Pr,sizeof(Receive_Data_Pr));                     //通过串口读取下位机发送过来的数据


    Receive_Data.rx[count] = Receive_Data_Pr[0];                                    //串口数据填入数组

    Receive_Data.Frame_Header = Receive_Data.rx[0];                                 //数据的第一位是帧头0X7B
    Receive_Data.Frame_Tail = Receive_Data.rx[23];                                  //数据的最后一位是帧尾0X7D

    if(Receive_Data_Pr[0] == FRAME_HEADER || count>0)                               //确保数组第一个数据为FRAME_HEADER
    {   
        count++;
    }
    else 
    {
  	    count=0;
    }
    if(count == 24)                                                                 //验证数据包的长度
    {
        count=0;                                                                    //为串口数据重新填入数组做准
        if(Receive_Data.Frame_Tail == FRAME_TAIL)                                       //验证数据包的帧尾
        {
            check=Check_Sum(22,READ_DATA_CHECK);                                        //BCC校验通过或者两组数据包交错
            if(check == Receive_Data.rx[22])  
            {
                error=0;                                                                //异或位校验成功
            }
            if(error == 0)
            {
                Receive_Data.Flag_Stop=Receive_Data.rx[1]; 
                Robot_Vel.X = Odom_Trans(Receive_Data.rx[2],Receive_Data.rx[3]);        //获取运动底盘X方向速度
                Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]);        //获取运动底盘Y方向速度
                Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]);        //获取运动底盘Z方向速度   

                Mpu6050_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8],Receive_Data.rx[9]);      //获取IMU的X轴加速度  
                Mpu6050_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10],Receive_Data.rx[11]);    //获取IMU的Y轴加速度
                Mpu6050_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12],Receive_Data.rx[13]);    //获取IMU的Z轴加速度
                Mpu6050_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14],Receive_Data.rx[15]);     //获取IMU的X轴角速度  
                Mpu6050_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16],Receive_Data.rx[17]);     //获取IMU的Y轴角速度  
                Mpu6050_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18],Receive_Data.rx[19]);     //获取IMU的Z轴角速度  

                //线性加速度单位转化，和STM32的IMU初始化的时候的量程有关,这里量程±2g=19.6m/s^2
                Mpu6050.linear_acceleration.x = Mpu6050_Data.accele_x_data / ACCEl_RATIO;
                Mpu6050.linear_acceleration.y = Mpu6050_Data.accele_y_data / ACCEl_RATIO;
                Mpu6050.linear_acceleration.z = Mpu6050_Data.accele_z_data / ACCEl_RATIO;
        
                //陀螺仪单位转化，和STM32的IMU初始化的时候的量程有关，这里IMU的陀螺仪的量程是±500°/s
                //因为机器人一般Z轴速度不快，降低量程可以提高精度
                Mpu6050.angular_velocity.x =  Mpu6050_Data.gyros_x_data * GYROSCOPE_RATIO;
                Mpu6050.angular_velocity.y =  Mpu6050_Data.gyros_y_data * GYROSCOPE_RATIO;
                Mpu6050.angular_velocity.z =  Mpu6050_Data.gyros_z_data * GYROSCOPE_RATIO;

                //Get the battery voltage
                //获取电池电压
                transition_16 = 0;
                transition_16 |=  Receive_Data.rx[20]<<8;
                transition_16 |=  Receive_Data.rx[21];  
                Power_voltage = transition_16/1000+(transition_16 % 1000)*0.001;    //单位转换毫伏(mv)->伏(v)
              
                return true;
            }
      
        }
    }
  return false;
}

/**************************************
功能: 循环获取下位机数据与发布话题
***************************************/
void turn_on_robot::Control()
{
    _Last_Time = ros::Time::now();
    while(ros::ok())
    {
        Sampling_Time = (_Now - _Last_Time).toSec(); 
        //获取时间间隔，用于积分速度获得位移(里程)
        _Now = ros::Time::now();

        //通过串口读取并校验下位机发送过来的数据，然后数据转换为国际单位
        if (true == Get_Sensor_Data_New())                      
        {
            //计算X方向的位移，单位：m
            Robot_Pos.X+=(Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * Sampling_Time;
            //计算Y方向的位移，单位：m
            Robot_Pos.Y+=(Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * Sampling_Time;
            //绕Z轴的角位移，单位：rad  
            Robot_Pos.Z+=Robot_Vel.Z * Sampling_Time;
            //通过IMU绕三轴角速度与三轴加速度计算三轴姿态
            Quaternion_Solution(Mpu6050.angular_velocity.x, Mpu6050.angular_velocity.y, Mpu6050.angular_velocity.z,\
                Mpu6050.linear_acceleration.x, Mpu6050.linear_acceleration.y, Mpu6050.linear_acceleration.z);

            Publish_Odom();                                                 //发布里程计话题
            Publish_ImuSensor();                                            //发布IMU话题    
            Publish_Voltage();                                              //发布电源电压话题

            _Last_Time = _Now;                                              //记录时间，用于计算时间间隔
        }
        ros::spinOnce();                                                    //循环等待回调函数
    }
}

/**************************************
功能: 构造函数, 只执行一次，用于初始化
***************************************/
turn_on_robot::turn_on_robot():Sampling_Time(0),Power_voltage(0)
{
    //清空数据
    memset(&Robot_Pos, 0, sizeof(Robot_Pos));
    memset(&Robot_Vel, 0, sizeof(Robot_Vel));
    memset(&Receive_Data, 0, sizeof(Receive_Data)); 
    memset(&Send_Data, 0, sizeof(Send_Data));
    memset(&Mpu6050_Data, 0, sizeof(Mpu6050_Data));

    ros::NodeHandle private_nh("~");                               
    private_nh.param<std::string>("usart_port_name",  usart_port_name,  "/dev/CP2102"); 
    private_nh.param<int>        ("serial_baud_rate", serial_baud_rate, 115200); 
    private_nh.param<std::string>("odom_frame_id",    odom_frame_id,    "odom_combined"); 
    private_nh.param<std::string>("robot_frame_id",   robot_frame_id,   "base_footprint");
    private_nh.param<std::string>("gyro_frame_id",    gyro_frame_id,    "imu_link"); 

    voltage_publisher = n.advertise<std_msgs::Float32>("PowerVoltage", 10);     //创建电池电压话题发布者
    odom_publisher = n.advertise<nav_msgs::Odometry>("odom", 50);               //创建里程计话题发布者
    imu_publisher = n.advertise<sensor_msgs::Imu>("imu", 20);                   //创建IMU话题发布者

    //速度控制命令订阅回调函数设置
    Cmd_Vel_Sub     = n.subscribe("cmd_vel",100, &turn_on_robot::Cmd_Vel_Callback, this); 
  
    try
    { 
        Stm32_Serial.setPort(usart_port_name); 
        Stm32_Serial.setBaudrate(serial_baud_rate); 
        serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
        Stm32_Serial.setTimeout(_time);
        Stm32_Serial.open(); 
    }
    catch (serial::IOException& e)
    {
        ROS_ERROR_STREAM("Please check the serial port cable! ");
    }
    if(Stm32_Serial.isOpen())
    {
        ROS_INFO_STREAM("zeus_s1_driver --->ok");
    }
}

/**************************************
功能: 析构函数，只执行一次，当对象结束其生命周期时系统会调用这个函数
***************************************/
turn_on_robot::~turn_on_robot()
{
    //结束前向下位机发送停止运动命令
    Send_Data.tx[0]=FRAME_HEADER;
    Send_Data.tx[1] = 0;  
    Send_Data.tx[2] = 0; 

    //机器人X轴的目标线速度 
    Send_Data.tx[4] = 0;     
    Send_Data.tx[3] = 0;  

    //机器人Y轴的目标线速度 
    Send_Data.tx[6] = 0;
    Send_Data.tx[5] = 0;  

    //机器人Z轴的目标角速度 
    Send_Data.tx[8] = 0;  
    Send_Data.tx[7] = 0;    
    Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK);
    Send_Data.tx[10]=FRAME_TAIL; 
    try
    {
        Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); 
    }
    catch (serial::IOException& e)   
    {
        ROS_ERROR_STREAM("Unable to send data through serial port"); 
    }
    Stm32_Serial.close();                                               //关闭串口  
    ROS_INFO_STREAM("Shutting down"); 
}

